//
// Created by breeze on 23/3/31.
//
#include <iostream>
#include <cmath>
#include "robot.h"
#include "func.h"

using namespace std;

TRobot::TRobot(int _number,double _x,double _y)
:number(_number),x(_x),y(_y)
{

}

void TRobot::Read(string &buff)
{
    // 将buff字符串拆分成子串存入strs容器中
    stringstream ss(buff);
    string substr;
    vector<string> strs;
    while (ss >> substr)
    {
        if (substr.find_first_not_of(' ') != std::string::npos)
            strs.push_back(substr);
    }


    this->platform_idx = stoi(strs[0]);//在idx工作台范围内

    //货物初始化(物品类型,时间价值，碰撞系数)
    this->foods.number = stoi(strs[1]);
    this->foods.ktime = stod(strs[2]);
    this->foods.kcoll = stod(strs[3]);
    //角速度
    this->rad_v = stod(strs[4]);
    this->line_v[0] = stod(strs[5]);
    this->line_v[1] = stod(strs[6]);
    this->direction = stod(strs[7]);
    this->x = stod(strs[8]);
    this->y = stod(strs[9]);

    is_busy = !this->work_queue.empty();
}

void TRobot::Goto(const double x_dst, const double y_dst)
{
    double rx = x;
    double ry = y;
    double rdis = radius;
    double rvx = line_v[0];
    double rvy = line_v[1];
    double tardirect = atan2(y_dst - ry, x_dst - rx); //机器人-工作台角度
    double direction = direction;        //机器人朝向
    double vdirecton = atan2(rvy, rvx) ;
    double direction_dplust = AngleDiff(direction,tardirect) / M_PI * 180; //朝向和目标的夹角
    double total_v = sqrt((rvy*rvy)+(rvx*rvx));
    double dist = Distance(rx, ry, x_dst, y_dst); // 计算当前距离

    v_set = 6;
    if (fabs(direction_dplust) < 3)
    {
        rotate_set = 0;
        v_set = 6;
    }
    else if (fabs(direction_dplust) < 8)
    {
        v_set = 6;
        if (direction_dplust > 0)
            rotate_set = +M_PI/6;
        else
            rotate_set = -M_PI/6;

    }
    else if (fabs(direction_dplust) < 45)
    {
        v_set = 6;
        if (direction_dplust > 0)
            rotate_set = +M_PI/3;
        else
            rotate_set = -M_PI/3;
    }
    else if (fabs(direction_dplust) < 90)
    {
        v_set = 4;             //速度高了
        if (direction_dplust > 0)
            rotate_set = +M_PI;
        else
            rotate_set = -M_PI;
    }
    else if (fabs(direction_dplust) < 180)
    {
        v_set = -1;
        if (direction_dplust>0)
            rotate_set = +M_PI;
        else
            rotate_set = -M_PI;
    }
}